#include <GSLAM/core/GSLAM.h>
#include <GSLAM/core/GPS.h>

using namespace GSLAM;


Point3d from_pixel_to_lla1(FramePtr fr, std::vector<double> pixel){

    auto PYR2Rotation=[](double pitch,double yaw,double roll)->GSLAM::SO3
    {
        GSLAM::SO3 body2world(0.707107,0.707107,0,0);
        GSLAM::SO3 camera2imu(0.5,0.5,0.5,0.5);
        GSLAM::SO3 imu2body;
        imu2body.FromEulerAngle(pitch,yaw,roll);
        return body2world*imu2body*camera2imu;
    };

    if(fr->getGPSNum()==0) return Point3d(0,0,-1e8);
    GSLAM::Point3d lla;
    if(!fr->getGPSLLA(lla)) return Point3d(0,0,-1e8);
    lla.y = lla.y * 180.0/3.1415926;
    lla.x = lla.x * 180.0/3.1415926;

    GSLAM::SE3 _local2ECEF;
    _local2ECEF.get_translation()=GSLAM::GPS<>::GPS2XYZ(lla.y,lla.x,lla.z);
    double D2R=3.1415926/180.;
    double lon=lla.x*D2R;
    double lat=lla.y*D2R;
    GSLAM::Point3d up(cos(lon)*cos(lat), sin(lon)*cos(lat), sin(lat));
    GSLAM::Point3d east(-sin(lon), cos(lon), 0);
    GSLAM::Point3d north=up.cross(east);
    double R[9]={east.x, north.x, up.x,
                 east.y, north.y, up.y,
                 east.z, north.z, up.z};
    _local2ECEF.get_rotation().fromMatrix(R);

    GSLAM::Point2d height_ground;
    if(!fr->getHeight2Ground(height_ground)){
        return lla;
    }

    GSLAM::Point3d pyr(-90,svar.GetDouble("yaw",0),0),pyrSigma(1e100,1e100,1e100);
    GSLAM::SE3 camera2local;
    //GSLAM::Point3d ecef;
    //fr->getGPSECEF(ecef);
    //camera2local.get_translation()=_local2ECEF.inverse()*ecef;
    if(!fr->getPitchYawRoll(pyr))
    {
        return lla;
    }
    camera2local.get_rotation()=PYR2Rotation(pyr.x,pyr.y,pyr.z);

    Point3d axis=camera2local.get_rotation()*fr->getCamera().UnProject(pixel[0],pixel[1]);
    double scale=height_ground.x/(fabs(axis.z)+0.0001);

    lla= GSLAM::GPS<>::XYZ2GPS(_local2ECEF*(axis*scale+camera2local.get_translation()));
    return lla;
}


Point3d from_pixel_to_lla(FramePtr fr, std::vector<double> pixel){

    auto PYR2Rotation=[](double pitch,double yaw,double roll)->GSLAM::SO3
    {
        GSLAM::SO3 body2world(0.707107,0.707107,0,0);
        GSLAM::SO3 camera2imu(0.5,0.5,0.5,0.5);
        GSLAM::SO3 imu2body;
        imu2body.FromEulerAngle(pitch,yaw,roll);

        SO3 p,y,r;
        p.FromEulerAngle(pitch,0,0);
        y.FromEulerAngle(0,yaw,0);
        r.FromEulerAngle(0,0,roll);
        imu2body=  y*r*p;

        return body2world*imu2body*camera2imu;
    };


    if(fr->getGPSNum()==0) return Point3d(0,0,-1e8);
    GSLAM::Point3d lla;
    if(!fr->getGPSLLA(lla)) return Point3d(0,0,-1e8);

    GSLAM::SE3 _local2ECEF;
    _local2ECEF.get_translation()=GSLAM::GPS<>::GPS2XYZ(lla.y,lla.x,lla.z);
    double D2R=3.1415926/180.;
    double lon=lla.x*D2R;
    double lat=lla.y*D2R;
    GSLAM::Point3d up(cos(lon)*cos(lat), sin(lon)*cos(lat), sin(lat));
    GSLAM::Point3d east(-sin(lon), cos(lon), 0);
    GSLAM::Point3d north=up.cross(east);
    double R[9]={east.x, north.x, up.x,
                 east.y, north.y, up.y,
                 east.z, north.z, up.z};
    _local2ECEF.get_rotation().fromMatrix(R);

    GSLAM::Point2d height_ground;
    if(!fr->getHeight2Ground(height_ground)){
        return lla;
    }

    GSLAM::Point3d pyr(-90,svar.GetDouble("yaw",0),0),pyrSigma(1e100,1e100,1e100);
    GSLAM::SE3 camera2local;
    GSLAM::Point3d ecef;
    fr->getGPSECEF(ecef);
    camera2local.get_translation()=_local2ECEF.inverse()*ecef;
    if(!fr->getPitchYawRoll(pyr))
    {
        return lla;
    }
    camera2local.get_rotation()=PYR2Rotation(pyr.x,pyr.y,pyr.z);

    Point3d axis=camera2local.get_rotation()*fr->getCamera().UnProject(pixel[0],pixel[1]);
    double scale=height_ground.x/(fabs(axis.z)+0.0001);

    lla= GSLAM::GPS<>::XYZ2GPS(_local2ECEF*(axis*scale+camera2local.get_translation()));
    return lla;
}

REGISTER_SVAR_MODULE(pixel2lla){
    jvar["from_pixel_to_lla"] = from_pixel_to_lla;
    jvar["from_pixel_to_lla1"] = from_pixel_to_lla1;

    sv::Class<Point3d>("Point3d")
            .construct<>()
            .construct<double,double,double>()
            .def("dot",&Point3d::dot)
            .def("norm",&Point3d::norm)
            .def("at",&Point3d::at)
            .def("__repr__",&Point3d::toString)
            .def("__str__",&Point3d::toString)
            .def("__add__",&Point3d::add)
            .def("__sub__",&Point3d::sub)
            .def("__mul__",&Point3d::mul)
            .def("__div__",&Point3d::div)
            .def_readwrite("x", &Point3d::x)
            .def_readwrite("y", &Point3d::y)
            .def_readwrite("z", &Point3d::z);
}
